#include "QCVGraph.h"

void QCVGraph::read(QString filename)
{
	QFile file(filename);
	
	if(file.open(QIODevice::ReadOnly | QIODevice::Text))
	{
		QTextStream in(&file);

		in>>numOfVertices;
		adjMatrix.create(numOfVertices,numOfVertices);
		predecessor.create(numOfVertices,1);
		distances.create(numOfVertices,1);
		mark.create(numOfVertices,1);

		//Read the adjacency matrix for the graph with +ve weights
		for(int i=0;i<numOfVertices;i++) 
		{
			for(int j=0;j<numOfVertices;j++) 
			{
				in>>adjMatrix(i,j);
			}
		}
	}	
	else
	{
		printf("Erreur QCVGraph: Impossible d'ouvrir le fichier de graph\n");
	}
}

void QCVGraph::read(cv::Mat_<float> & adjMat)
{
	this->adjMatrix=adjMat.clone();
	numOfVertices=adjMatrix.rows;
	predecessor.create(numOfVertices,1);
	distances.create(numOfVertices,1);
	mark.create(numOfVertices,1);
}

void QCVGraph::initialize(int src)
{
	source=src;
	mark.setTo(false);
	predecessor.setTo(-1);
	distances.setTo(INF);
	distances(source)=0;
}

int QCVGraph::getClosestUnmarkedNode()
{
	float minDistance = INF;
	int closestUnmarkedNode=0;
	for(int i=0;i<numOfVertices;i++) 
	{
		if((!mark(i)) && ( minDistance >= distances(i))) 
		{
			minDistance = distances(i);
			closestUnmarkedNode = i;
		}
	}
	return closestUnmarkedNode;
}

void QCVGraph::dijkstra()
{
	int closestUnmarkedNode=0;
	int count=0;
	while(count < numOfVertices) 
	{
		closestUnmarkedNode = getClosestUnmarkedNode();
		
		mark(closestUnmarkedNode) = true;
		for(int i=0;i<numOfVertices;i++) 
		{
			if((!mark(i)) && (adjMatrix(closestUnmarkedNode,i)>0) ) 
			{
				if(distances(i) > distances(closestUnmarkedNode)+adjMatrix(closestUnmarkedNode,i)) 
				{
					distances(i) = distances(closestUnmarkedNode)+adjMatrix(closestUnmarkedNode,i);
					predecessor(i) = closestUnmarkedNode;
				}
			}
		}
		count++;
	}
}

void QCVGraph::setSource(int src)
{
	source=src;
}

void QCVGraph::stdPath(int node)
{
	if(node == source)
		printf("%d ",node);
	else if(predecessor(node) == -1)
		printf("Erreur pas de chemin de %d vers %d \n",source,node);
	else 
	{
		stdPath(predecessor(node));
		printf("%d ",node);
	}
}

void QCVGraph::stdPrintPath(int src,int des)
{
	if( (src>=0 && src<numOfVertices) && (des>=0 && des<numOfVertices))
	{
		initialize(src);
		dijkstra();
		stdPath(des);

		printf(" dist=%f\n",distances(des));
	}
}

void QCVGraph::stdPrint()
{
	printf("---------------------------------------------------------\n");
	printf("Matrice d'adjacences:\n");
	if(!adjMatrix.empty()){
	for(int i=0;i<numOfVertices;i++) 
	{
		for(int j=0;j<numOfVertices;j++) 
		{
			printf("%4.1f ",adjMatrix(i,j));
		}
		printf("\n");
	}}
}

void QCVGraph::recPath(int des,std::vector<int> & p)
{
	if(des == source)
		p.push_back(des);
	else if(predecessor(des) == -1)
		p.clear();
	else 
	{
		stdPath(predecessor(des));
		p.push_back(des);
	}
}

void QCVGraph::path(int src,int des,std::vector<int> & p)
{
	p.clear();
	initialize(src);
	dijkstra();
	recPath(des,p);
}

void QCVGraph::path(int src,int des,std::vector<int> & p,float * dist)
{
	p.clear();	
	initialize(src);
	dijkstra();
	recPath(des,p);
	*dist=distances(des);
}

float QCVGraph::dist(int src,int des)
{
	float d=0;
	std::vector<int> p;
	path(src,des,p,&d);
	return d;
}

void QCVGraph::getCostMat(cv::Mat_<float> & costMat)
{
	costMat.create(numOfVertices,numOfVertices);
	for(int i=0;i<numOfVertices;i++) 
	{
		for(int j=0;j<numOfVertices;j++) 
		{
			costMat(i,j)=dist(i,j);
		}
	}
}
